/*
**Author erow
**
*/
#include <iostream>
#include <sys/time.h>
#include <math.h>
#include <unistd.h>
#include <stdio.h>

#include "robotModel.h"
#include <eigen3/Eigen/Eigen>

#include "gaitStep.h"
#include "gaitManager.h"
#include <pthread.h>
#include "include/visionManager.h"
#include "Object.h"
#include "struct_vision.h"
STRUCT_VISION::FACE_INFO g_face_info;

transitHub* port=transitHub::getInstance();
string logger::m_path="log/";
robotModel r;
gaitManager g_gait_manager;
visionManager g_vison_manager;
void *gait_loop(void *threadid)
{
int tid;
tid = (int)threadid;
g_gait_manager.loop();
pthread_exit(NULL);
}
void *vision_loop(void *threadid)
{
int tid;
tid = (int)threadid;
g_vison_manager.loop();
pthread_exit(NULL);
}
void move_head()
{
    int data[20]={0};
    r.modifyData(data);
    float t_s=0; int t_c=0;
    while(1)
    {
    int t=0;

    if(fa.isNew(t))
    {//find face
         t=fa.readData(&g_face_info);
        if(g_gait_manager.p_name_gait!="")
        {
            g_gait_manager.p_name_gait="";
            port->doLoopTx(data);
            sleep(1);
        }
        t_c=0;
        float distance_pic_x=g_face_info.p_face_x-g_face_info.p_picture_width/2;
        float distance_pic_y=g_face_info.p_face_y-g_face_info.p_picture_length/2;
        static float ry=0,rz=0;

        ry=distance_pic_y*0.01;
        rz=distance_pic_x*0.01;
         cout<<ry<<","<<rz<<endl;
        r.setHead(ry,rz);
        r.setLeg(0,0,0,21,1);r.setLeg(0,0,0,21,2);
        r.getData(data);
        port->doLoopTx(data);

    }
    else
    {
    t_c++;
    continue;
        if(g_gait_manager.p_name_gait!="shakehand.gait"&&t_c>30)
            g_gait_manager.p_name_gait="shakehand.gait";
    }
    usleep(30*1000);
    }
}
void TestHand(int *);
int main(int arg,char** args){

    //port:1,baud_rate:57600
   // cin>>port>>way;

   logger::m_path="log/";
   logger::m_level=WARN;

    while(!port->portOpen(1,57600)){
            cout<<"fail!\n";
    cout<<"(port way)//ttyS$port"<<endl;
    return 1;
    }

    gaitStep t_shake_hand("shakehand.gait",28);
    float t_s=0;float t_ds=0.042;
    int data[20]={0};r.modifyData(data);
    //TestHand(data);
//    while(1)
//    {
//
//        r.setHand(-(1+t_s),-2.2,21,side::left);
//        r.setHand(1+t_s,-2.2,21,side::right);
//
//        r.setHand(1.5,0,21,side::left);
//        r.setHand(-1.5,0,21,side::right);
//        r.setLeg(0,0,0,21,side::left);
//        r.setLeg(0,0,0,21,side::right);
//        r.getData(data);
//        t_shake_hand.pushData(data);
//        port->doLoopTx(data);
//        if(t_s<0){
//            t_ds=0.1;
//            break;
//        }
//        if(t_s>=0.6)
//            t_ds=-t_ds;
//        t_s+=t_ds;
//    }
//    t_shake_hand.saveFile();

    pthread_t id,id_vision;
    pthread_create(&id,NULL,(void *) gait_loop,NULL);
    pthread_create(&id_vision,NULL,(void *) vision_loop,NULL);
    move_head();
    return 0;
}
void TestHand(int *data)
{
    port->doLoopTx(data);
    usleep(2000*1000);
    r.modifyData(data);
    float x,y,z,len;
    float s=1;
    float t_k=1;
    while(1)
    {
        cout<<"type rx,ry,len\n";
        r.getData(data);
        float x,y,z,len;
        cin>>x>>y>>len;
        r.setHand(x,y,len,side::right);
        r.setHand(x,y,len,side::left);
        //r.showParam(side::right);
        port->doLoopTx(data);
        usleep(20*1000);
    }

}

//void SerialTest()
//{
//     Serial ss("/dev/ttyUSB0",57600);
//    char m[200];
//    int num=0;
//    sleep(3);
//    cout<<"start dev"<<endl;
//
//    while(1){
//    while(ss.ReadSerial(m,1))
//     {
//         cout<<m;
//        num=0;
//     }
//     if(num==0)
//        ss.WriteSerial("asd\n",4);
//     num++;
//    //usleep(1050);
//
//    }
//}
